import rclpy
import rclpy.time
import math,time
from rclpy.node import Node #Node 类
from geometry_msgs.msg import PoseStamped,Pose
from nav2_simple_commander.robot_navigator import BasicNavigator,TaskResult
from tf2_ros import TransformListener,Buffer
from tf_transformations import euler_from_quaternion,quaternion_from_euler#四元数转换为欧拉角
from partol_voice.srv import SpeechText
from sensor_msgs.msg import Image   #消息接口
from cv_bridge import CvBridge      #转换图像格式
import cv2                          #保存图像



class PartolNode(BasicNavigator):
    def __init__(self, node_name='partol_node'):
        super().__init__(node_name)
        #生命相关参数
        self.declare_parameter('init_point',[0.0,0.0,0.0])
        self.declare_parameter('target_point',[0.0,0.0,0.0,  1.0,1.0,1.57])     
        self.declare_parameter('img_save_path','')
        self.init_pose_ = self.get_parameter('init_point').value
        self.target_pose_ = self.get_parameter('target_point').value
        self.img_save_path = self.get_parameter('img_save_path').value
        self.cv_bridge_ = CvBridge()
        self.latest_img_ = None
        #图像处理订阅端---处理图像
        self.img_sub_ = self.create_subscription(Image,'/camera_sensor/image_raw',self.img_callback,10)
        #实时位置获取 TF 相关定义
        self.buffer_ = Buffer() #存放监听到的机器人坐标
        self.listener_ = TransformListener(self.buffer_,self)
        #语音合成客户端
        self.speech_client_ = self.create_client(SpeechText,'speech_text')
        self.count = 0
    def img_callback(self,msg):
        self.latest_img_ = msg

    def record_img(self):
        if self.latest_img_ is not None:
            #文件名字设定
            pose = self.get_current_pose()
            cv_image = self.cv_bridge_.imgmsg_to_cv2(self.latest_img_)
            # cv2.imwrite(
            #     f'{self.img_save_path}img_{pose.translation.x:3.2f}_{pose.translation.y:3.2f}.png',cv_image
            # )
            cv2.imwrite(
                f'{self.img_save_path}img_{self.count}.png',cv_image
            )
            self.count +=1

    def speech_text(self,text):
        '''
        调用服务器播放语音
        '''
        while not self.speech_client_.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('语音合成服务器未上线，等待中...')
        request = SpeechText.Request()
        request.text = text
        future = self.speech_client_.call_async(request)
        rclpy.spin_until_future_complete(self,future)
        if future.result() is not None:
            response = future.result()
            if response.result == True:
                self.get_logger().info(f'语音合成成功{text}')
            else:
                self.get_logger().warn(f'语音合成失败{text}')
        else:
            self.get_logger().warn(f'语音合成服务响应失败')

        
    def get_pose_by_xyyaw(self,x,y,yaw):
        '''
        return PoseStamped对象
        '''
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.pose.position.x=x
        pose.pose.position.y=y
        #返回顺序为 x y z w
        quat = quaternion_from_euler(0,0,yaw)
        pose.pose.orientation.x=quat[0]
        pose.pose.orientation.y=quat[1]
        pose.pose.orientation.z=quat[2]
        pose.pose.orientation.w=quat[3]     
        return pose

    def init_robot_pose(self):
        '''
        初始化机器人位姿
        '''
        self.init_pose_ = self.get_parameter('init_point').value

        init_pose = self.get_pose_by_xyyaw(self.init_pose_[0],self.init_pose_[1],self.init_pose_[2])

        self.setInitialPose(init_pose) #设置初始状态

        self.waitUntilNav2Active()#等待导航激活

    def get_target_pose(self):
        '''
        通过参数值获取目标点的集合
        '''
        points = []
        self.target_points_ = self.get_parameter('target_point').value
        for index in range(int(len(self.target_points_)/3)):
            x = self.target_points_[index*3]
            y = self.target_points_[index*3+1]
            yaw=self.target_points_[index*3+2]
            points.append([x,y,yaw])
            self.get_logger().info(f'获取到目标点: {index}->({x},{y},{yaw})')
        return points
    def nav_to_pose(self,target_pose):
        '''
        导航到目标点
        '''
        self.waitUntilNav2Active()
        result = self.goToPose(target_pose)
        while not self.isTaskComplete():
            feedback = self.getFeedback()
            if feedback:
                self.get_logger().info(f'剩余距离：{feedback.distance_remaining}')
            # result = self.getResult()
            # if result == TaskResult.SUCCEEDED:
            #     self.get_logger().info('导航结果: 成功@@@')
            # elif result == TaskResult.CANCELED:
            #     self.get_logger().info('导航结果: 被取消')
            # else:
            #     self.get_logger().error('导航结果: 返回状态无效')
            

    def get_current_pose(self):
        '''
        获取机器人当前的位姿
        '''
        while rclpy.ok():
            try:
                tf = self.buffer_.lookup_transform(
                    'map','base_footprint',rclpy.time.Time(seconds=0),rclpy.time.Duration(seconds=1))
                transform = tf.transform
                rotation_euler = euler_from_quaternion([
                    transform.rotation.x,
                    transform.rotation.y,
                    transform.rotation.z,
                    transform.rotation.w
                ])
                self.get_logger().info(f'平移：{transform.translation},旋转四元数：{transform.rotation},旋转欧拉角度：{rotation_euler}')
                return transform
            except Exception as e:
                self.get_logger().info(f'不能获取坐标变换，原因：{str(e)}')
def main():
    rclpy.init()
    partol=PartolNode()
    partol.speech_text('正在准备初始化位置')
    # rclpy.spin(partol)
    partol.init_robot_pose()#初始化机器人位置
    partol.speech_text('初始化位置完成')
    while rclpy.ok():# 只要没收到终止信号，就一直循环
        points = partol.get_target_pose()
        for point in points:
            x,y,yaw = point[0],point[1],point[2]
            target_pose = partol.get_pose_by_xyyaw(x,y,yaw)
            partol.speech_text(f'正在准备前往{x},{y}目标点')
            partol.nav_to_pose(target_pose)
            partol.speech_text(f'已经到达目标点{x},{y},正在准备记录图像')
            partol.record_img()
            partol.speech_text(f'图像记录完成')
    rclpy.shutdown()
